#include <stdio.h>
#include <string.h>
#include <semaphore.h>
#include <stdlib.h>
#include <pthread.h>
#include <sys/time.h>
#include <sys/prctl.h>
#include <imp/imp_common.h>
#include <imp/imp_log.h>
#include <system/vbm.h>
#include <imp/imp_utils.h>

#include <icommon.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
#include "imp_video_common.h"

#define TAG	"VIDEOCOMMON"

#define VIDEO_VBM_INFO_MAX_NUM      16

typedef struct {
    IMPAlloc alloc;
    int      b_used;
    uint32_t *vaddr;
} video_vbm_info_t;

static video_vbm_info_t *g_video_vbm_info = NULL;

void *video_vbm_malloc(int index, int size, int align)
{
    int i = 0;
	IMPAlloc alloc;
	void *vaddr = NULL;
	int poolId, ret;

	if (g_video_vbm_info == NULL) {
        g_video_vbm_info = calloc(VIDEO_VBM_INFO_MAX_NUM, sizeof(video_vbm_info_t));
        if (g_video_vbm_info == NULL) {
            IMP_LOG_ERR(TAG, "init video vbm info failed:%s\n", strerror(errno));
            goto err_calloc_g_video_vbm_info;
        }
    }

    for (i = 0; i < VIDEO_VBM_INFO_MAX_NUM; i++) {
        if (g_video_vbm_info[i].b_used == 0) {
            break;
        }
    }

    if (i >= VIDEO_VBM_INFO_MAX_NUM) {
        IMP_LOG_ERR(TAG, "can't find unused alloc info elem\n");
        goto err_no_alloc_info_elem;
    }

	poolId = IMP_Encoder_GetPool(index);
	if (poolId < 0) {
		if (IMP_Alloc(&alloc, size + align, "video") < 0) {
			IMP_LOG_ERR(TAG, "[%s][%d] IMP_Alloc failed size = %d \n",__func__, __LINE__, (size+align));
			goto err_vbm_alloc;
		}
	} else {
		ret = IMP_PoolAlloc(poolId, &alloc, (size+align), "video");
		if(ret < 0){
			IMP_LOG_ERR(TAG, "[%s][%d] IMP_PoolAlloc failed size = %d \n",__func__, __LINE__, (size+align));
			goto err_vbm_alloc;
		}
	}

	vaddr = (void *)c_align(alloc.info.vaddr, align, align * 4);
    memcpy(&g_video_vbm_info[i].alloc, &alloc, sizeof(IMPAlloc));
    g_video_vbm_info[i].vaddr = vaddr;
    g_video_vbm_info[i].b_used = 1;
#if 0
	IMP_LOG_INFO(TAG, "%s(%d):IMPAlloc Addr=0x%08x\n", __func__, __LINE__, (intptr_t)vaddr - sizeof(IMPAlloc));
	IMP_LOG_INFO(TAG, "%s(%d):alloc.info.vaddr = 0x%08x\n", __func__, __LINE__, alloc.info.vaddr);
	IMP_LOG_INFO(TAG, "%s(%d):alloc.info.paddr = 0x%08x\n", __func__, __LINE__, alloc.info.paddr);
	IMP_LOG_INFO(TAG, "%s(%d):alloc.info.length = %d\n", __func__, __LINE__, alloc.info.length);
	IMP_LOG_INFO(TAG, "%s(%d):alloc.info.ref_cnt = %d\n", __func__, __LINE__, alloc.info.ref_cnt);
	IMP_LOG_INFO(TAG, "%s(%d):alloc.info.mem_attr = %d\n", __func__, __LINE__, alloc.info.mem_attr);
#endif

	return vaddr;

err_vbm_alloc:
err_no_alloc_info_elem:
err_calloc_g_video_vbm_info:
	return NULL;
}

void video_vbm_free(int index, void *vaddr)
{
	int poolId;

    if (g_video_vbm_info == NULL) {
        IMP_LOG_WARN(TAG, "video vbm info han't been init\n");
    }

	if (vaddr) {
        int i = 0;

        for (i = 0; i < VIDEO_VBM_INFO_MAX_NUM; i++) {
            if ((vaddr == g_video_vbm_info[i].vaddr) && (g_video_vbm_info[i].b_used == 1)) {
                break;
            }
        }

        if (i >= VIDEO_VBM_INFO_MAX_NUM) {
            IMP_LOG_WARN(TAG, "%s:can't free vaddr=%x\n", __func__);
        }
#if 0
		IMP_LOG_INFO(TAG, "%s(%d):IMPAlloc Addr=0x%08x\n", __func__, __LINE__, (intptr_t)vaddr - sizeof(IMPAlloc));
		IMP_LOG_INFO(TAG, "%s(%d):palloc->info.vaddr = 0x%08x\n", __func__, __LINE__, palloc->info.vaddr);
		IMP_LOG_INFO(TAG, "%s(%d):palloc->info.paddr = 0x%08x\n", __func__, __LINE__, palloc->info.paddr);
		IMP_LOG_INFO(TAG, "%s(%d):palloc->info.length = %d\n", __func__, __LINE__, palloc->info.length);
		IMP_LOG_INFO(TAG, "%s(%d):palloc->info.ref_cnt = %d\n", __func__, __LINE__, palloc->info.ref_cnt);
		IMP_LOG_INFO(TAG, "%s(%d):palloc->info.mem_attr = %d\n", __func__, __LINE__, palloc->info.mem_attr);
#endif
		poolId = IMP_Encoder_GetPool(index);
		if (poolId < 0) {
			IMP_Free(&g_video_vbm_info[i].alloc, (void *)(g_video_vbm_info[i].alloc.info.vaddr));
		} else {
			IMP_PoolFree(poolId, &g_video_vbm_info[i].alloc, (void *)(g_video_vbm_info[i].alloc.info.vaddr));
		}
        memset(&g_video_vbm_info[i], 0, sizeof(video_vbm_info_t));
	}
}

intptr_t video_vbm_virt_to_phys(intptr_t vaddr)
{
	return (intptr_t)IMP_Virt_to_Phys((void *)vaddr);
}

intptr_t video_vbm_phys_to_virt(intptr_t paddr)
{
	return (intptr_t)IMP_Phys_to_Virt((void *)paddr);
}

int video_sem_timedwait(sem_t *sem, const struct timespec *abs_timeout)
{
	struct timespec rltimeSpec;
	if (clock_gettime(CLOCK_REALTIME, &rltimeSpec) < 0) {
		IMP_LOG_INFO(TAG, "clock_gettime CLOCK_REALTIME failed\n");
		goto err_clock_gettime;
	}

	rltimeSpec.tv_sec = rltimeSpec.tv_sec + abs_timeout->tv_sec + (rltimeSpec.tv_nsec + abs_timeout->tv_nsec) / 1000000000;
	rltimeSpec.tv_nsec = (rltimeSpec.tv_nsec + abs_timeout->tv_nsec) % 1000000000;

	return sem_timedwait(sem, &rltimeSpec);

err_clock_gettime:
	return -1;
}

void imp_video_push_queue_head(imp_video_list_queue_t *list_queue, imp_video_list_t *head, imp_video_list_t *new)
{
	pthread_mutex_lock(&list_queue->mutex);
	new->next = head->next;
	new->prev = head;
	head->next->prev = new;
	head->next = new;
	pthread_mutex_unlock(&list_queue->mutex);
}

/* push queue to end */
void imp_video_push_queue_tail(imp_video_list_queue_t *list_queue, imp_video_list_t *head, imp_video_list_t *new)
{
	pthread_mutex_lock(&list_queue->mutex);
	new->next = head;
	new->prev = head->prev;
	head->prev->next = new;
	head->prev = new;
	pthread_mutex_unlock(&list_queue->mutex);
}

/* push queue from head */
void imp_video_pop_queue(imp_video_list_queue_t *list_queue, imp_video_list_t *head, imp_video_list_t **new)
{
	pthread_mutex_lock(&list_queue->mutex);
	if (head->next == head) {
		*new = NULL;
	} else {
		*new = head->next;
		head->next = head->next->next;
		head->next->prev = head;
		(*new)->next = (*new)->prev = *new;
	}
	pthread_mutex_unlock(&list_queue->mutex);
}

imp_video_container_t *imp_video_container_init(uint32_t elem_num, uint32_t elem_data_size)
{
	int i = 0;
	imp_video_container_t *pcontainer = NULL;

	pcontainer = calloc(1, sizeof(imp_video_container_t));
	if (pcontainer == NULL) {
		IMP_LOG_ERR(TAG, "calloc imp_video_container_t failed\n");
		goto err_calloc_imp_video_container;
	}

	pcontainer->pelem = calloc(elem_num, sizeof(imp_video_list_elem_t));
	if (pcontainer->pelem == NULL) {
		IMP_LOG_ERR(TAG, "calloc imp_video_list_elem_t failed\n");
		goto err_calloc_imp_video_list_elem;
	}

	for (i = 0; i < elem_num; i++) {
		pcontainer->pelem[i].data = calloc(1, elem_data_size);
		if (pcontainer->pelem[i].data == NULL) {
			IMP_LOG_ERR(TAG, "calloc pcontainer->pelem[%d].data failed\n", i);
			goto err_calloc_pcontainer_pelem;
		}
		pcontainer->pelem[i].dataSize = elem_data_size;
		pcontainer->pelem[i].free_data = NULL;
		pcontainer->pelem[i].list.prev = pcontainer->pelem[i].list.next = &pcontainer->pelem[i].list;
	}
	pcontainer->pelemCnt = elem_num;

	pthread_mutex_init(&pcontainer->freeListQueue.mutex, NULL);
	pcontainer->freeListQueue.queue.prev = (pcontainer->freeListQueue.queue.next = &pcontainer->freeListQueue.queue);
	pthread_mutex_init(&pcontainer->busyListQueue.mutex, NULL);
	pcontainer->busyListQueue.queue.prev = (pcontainer->busyListQueue.queue.next = &pcontainer->busyListQueue.queue);
	pthread_mutex_init(&pcontainer->doneListQueue.mutex, NULL);
	pcontainer->doneListQueue.queue.prev = (pcontainer->doneListQueue.queue.next = &pcontainer->doneListQueue.queue);

	for (i = 0; i < elem_num; i++) {
		imp_video_push_queue_tail(&pcontainer->freeListQueue, &pcontainer->freeListQueue.queue, &pcontainer->pelem[i].list);
	}

	return pcontainer;

err_calloc_pcontainer_pelem:
	for (--i; i >= 0; i--) {
		free(pcontainer->pelem[i].data);
		pcontainer->pelem[i].data = NULL;
	}
	free(pcontainer->pelem);
	pcontainer->pelem = NULL;
err_calloc_imp_video_list_elem:
	free(pcontainer);
	pcontainer = NULL;
err_calloc_imp_video_container:
	return NULL;
}

void imp_video_container_deinit(imp_video_container_t *pcontainer)
{
	if (pcontainer) {
		imp_video_list_queue_t *list_queue = NULL;
		imp_video_list_t *new = NULL;
		imp_video_list_elem_t *elem = NULL;
		int i = 0;
		for (i = 0; i < 3; i++) {
			list_queue = (i == 0) ? &pcontainer->freeListQueue : ((i == 1) ? &pcontainer->busyListQueue : &pcontainer->doneListQueue);
			while (1) {
				imp_video_pop_queue(list_queue, &list_queue->queue, &new);
				if (new == NULL) {
					pthread_mutex_destroy(&list_queue->mutex);
					list_queue->queue.prev = list_queue->queue.next = &list_queue->queue;
					break;
				} else {
					elem = container_of(new, imp_video_list_elem_t, list);
					if (elem->data && elem->free_data) {
						elem->free_data(elem->data);
						new->prev = new->next = new;
					}
				}
			}
		}
		for (i = pcontainer->pelemCnt - 1; i >= 0; i--) {
			free(pcontainer->pelem[i].data);
			pcontainer->pelem[i].data = NULL;
			pcontainer->pelem[i].dataSize = 0;
		}
		free(pcontainer->pelem);
		free(pcontainer);
	}
}

/****************************************************************************/
/****************************************************************************/
static void* Rtos_Malloc(size_t zSize)
{
  return malloc(zSize);
}

/****************************************************************************/
static void Rtos_Free(void* pMem)
{
  free(pMem);
}

/****************************************************************************/
static void* Rtos_Memset(void* pDst, int iVal, size_t zSize)
{
  return memset(pDst, iVal, zSize);
}

/****************************************************************************/
static AL_MUTEX Rtos_CreateMutex()
{
  pthread_mutex_t* pMutex = (AL_MUTEX)Rtos_Malloc(sizeof(pthread_mutex_t));

  if(pMutex)
  {
    pthread_mutexattr_t MutexAttr;
    pthread_mutexattr_init(&MutexAttr);
    pthread_mutexattr_settype(&MutexAttr, PTHREAD_MUTEX_RECURSIVE);
    pthread_mutex_init(pMutex, &MutexAttr);
  }
  return (AL_MUTEX)pMutex;
}

/****************************************************************************/
static void Rtos_DeleteMutex(AL_MUTEX Mutex)
{
  pthread_mutex_t* pMutex = (pthread_mutex_t*)Mutex;

  if(pMutex)
  {
    pthread_mutex_destroy(pMutex);
    Rtos_Free(pMutex);
  }
}

/****************************************************************************/
static bool Rtos_GetMutex(AL_MUTEX Mutex)
{
  pthread_mutex_t* pMutex = (pthread_mutex_t*)Mutex;

  if(!pMutex)
    return false;

  if(pthread_mutex_lock(pMutex) < 0)
    return false;

  return true;
}

/****************************************************************************/
static bool Rtos_ReleaseMutex(AL_MUTEX Mutex)
{
  if(!Mutex)
    return false;

  if((pthread_mutex_unlock((pthread_mutex_t*)Mutex)) < 0)
    return false;

  return true;
}

/****************************************************************************/
static AL_SEMAPHORE Rtos_CreateSemaphore(int iInitialCount)
{
  sem_t* pSem = (sem_t*)Rtos_Malloc(sizeof(sem_t));

  if(pSem)
    sem_init(pSem, 0, iInitialCount);

  return (AL_SEMAPHORE)pSem;
}

/****************************************************************************/
static void Rtos_DeleteSemaphore(AL_SEMAPHORE Semaphore)
{
  sem_t* pSem = (sem_t*)Semaphore;

  if(pSem)
  {
    sem_destroy(pSem);
    Rtos_Free(pSem);
  }
}

/****************************************************************************/
static bool Rtos_GetSemaphore(AL_SEMAPHORE Semaphore, uint32_t Wait)
{
  sem_t* pSem = (sem_t*)Semaphore;

  if(!pSem)
    return false;

  int ret;

  if(Wait == AL_NO_WAIT)
  {
    do
    {
      ret = sem_trywait(pSem);
    }
    while(ret == -1 && errno == EINTR);

    return ret == 0;
  }
  else if(Wait == AL_WAIT_FOREVER)
  {
    do
    {
      ret = sem_wait(pSem);
    }
    while(ret == -1 && errno == EINTR);

    return ret == 0;
  }
  else
  {
    struct timespec Ts;
    Ts.tv_sec = Wait / 1000;
    Ts.tv_nsec = (Wait % 1000) * 1000000;

    do
    {
      ret = sem_timedwait(pSem, &Ts);
    }
    while(ret == -1 && errno == EINTR);

    return ret == 0;
  }

  return true;
}

/****************************************************************************/
static bool Rtos_ReleaseSemaphore(AL_SEMAPHORE Semaphore)
{
  sem_t* pSem = (sem_t*)Semaphore;

  if(!pSem)
    return false;

  sem_post(pSem);
  return true;
}

/****************************************************************************/
static AL_EVENT Rtos_CreateEvent(bool bInitialState)
{
  evt_t* pEvt = (evt_t*)Rtos_Malloc(sizeof(evt_t));

  if(pEvt)
  {
    pthread_mutex_init(&pEvt->Mutex, 0);
    pthread_cond_init(&pEvt->Cond, 0);
    pEvt->bSignaled = bInitialState;
  }
  return (AL_EVENT)pEvt;
}

/****************************************************************************/
static void Rtos_DeleteEvent(AL_EVENT Event)
{
  evt_t* pEvt = (evt_t*)Event;

  if(pEvt)
  {
    pthread_cond_destroy(&pEvt->Cond);
    pthread_mutex_destroy(&pEvt->Mutex);
  }
  Rtos_Free(pEvt);
}

/****************************************************************************/
static bool Rtos_WaitEvent(AL_EVENT Event, uint32_t Wait)
{
  evt_t* pEvt = (evt_t*)Event;

  if(!pEvt)
    return false;

  bool reachedDeadline = false;

  pthread_mutex_lock(&pEvt->Mutex);

  if(Wait == AL_WAIT_FOREVER)
  {
    while(!pEvt->bSignaled)
      pthread_cond_wait(&pEvt->Cond, &pEvt->Mutex);
  }
  else
  {
    struct timeval now;
    gettimeofday(&now, NULL);

    struct timespec deadline;
    uint64_t uWaitNsec = (now.tv_usec + 1000ULL * Wait) * 1000ULL;
    deadline.tv_sec = (uWaitNsec / 1000000000ULL) + now.tv_sec;
    deadline.tv_nsec = uWaitNsec % 1000000000ULL;

    while(!reachedDeadline && !pEvt->bSignaled)
      reachedDeadline = (pthread_cond_timedwait(&pEvt->Cond, &pEvt->Mutex, &deadline) == ETIMEDOUT);
  }

  if(!reachedDeadline)
    pEvt->bSignaled = false;

  pthread_mutex_unlock(&pEvt->Mutex);
  return !reachedDeadline;
}

/****************************************************************************/
static bool Rtos_SetEvent(AL_EVENT Event)
{
  evt_t* pEvt = (evt_t*)Event;
  pthread_mutex_lock(&pEvt->Mutex);
  pEvt->bSignaled = true;
  bool bRet = pthread_cond_signal(&pEvt->Cond) == 0;
  pthread_mutex_unlock(&pEvt->Mutex);
  return bRet;
}

/****************************************************************************/
bool Fifo_Init(App_Fifo* pFifo, size_t zMaxElem)
{
  pFifo->m_zMaxElem = zMaxElem + 1;
  pFifo->m_zTail = 0;
  pFifo->m_zHead = 0;
  pFifo->m_iBufNumber = 0;
  pFifo->m_isDecommited = false;

  size_t zElemSize = pFifo->m_zMaxElem * sizeof(void*);
  pFifo->m_ElemBuffer = (void**)Rtos_Malloc(zElemSize);

  if(!pFifo->m_ElemBuffer)
    return false;
  Rtos_Memset(pFifo->m_ElemBuffer, 0xCD, zElemSize);

  pFifo->hEvent = Rtos_CreateEvent(0);

  if(!pFifo->hEvent)
  {
    Rtos_Free(pFifo->m_ElemBuffer);
    return false;
  }

  pFifo->hSpaceSem = Rtos_CreateSemaphore(zMaxElem);
  pFifo->hMutex = Rtos_CreateMutex();

  if(!pFifo->hSpaceSem)
  {
    Rtos_DeleteEvent(pFifo->hEvent);
    Rtos_Free(pFifo->m_ElemBuffer);
    return false;
  }

  return true;
}

/****************************************************************************/
void Fifo_Deinit(App_Fifo* pFifo)
{
  Rtos_Free(pFifo->m_ElemBuffer);
  Rtos_DeleteEvent(pFifo->hEvent);
  Rtos_DeleteSemaphore(pFifo->hSpaceSem);
  Rtos_DeleteMutex(pFifo->hMutex);
}

/****************************************************************************/
bool Fifo_Queue(App_Fifo* pFifo, void* pElem, uint32_t uWait)
{
  if(!Rtos_GetSemaphore(pFifo->hSpaceSem, uWait))
    return false;

  Rtos_GetMutex(pFifo->hMutex);
  pFifo->m_ElemBuffer[pFifo->m_zTail] = pElem;
  pFifo->m_zTail = (pFifo->m_zTail + 1) % pFifo->m_zMaxElem;
  ++pFifo->m_iBufNumber;
  Rtos_SetEvent(pFifo->hEvent);
  Rtos_ReleaseMutex(pFifo->hMutex);

  /* new item was added in the queue */
  return true;
}

/****************************************************************************/
void* Fifo_Dequeue(App_Fifo* pFifo, uint32_t uWait)
{
  /* wait if no items */
  Rtos_GetMutex(pFifo->hMutex);
  bool failed = false;

  while(true)
  {
    if(pFifo->m_iBufNumber > 0)
      break;

    if(failed || pFifo->m_isDecommited)
    {
      Rtos_ReleaseMutex(pFifo->hMutex);
      return NULL;
    }

    Rtos_ReleaseMutex(pFifo->hMutex);

    if(!Rtos_WaitEvent(pFifo->hEvent, uWait))
      failed = true;

    Rtos_GetMutex(pFifo->hMutex);
  }

  void* pElem = pFifo->m_ElemBuffer[pFifo->m_zHead];
  pFifo->m_zHead = (pFifo->m_zHead + 1) % pFifo->m_zMaxElem;
  --pFifo->m_iBufNumber;
  Rtos_ReleaseMutex(pFifo->hMutex);

  /* new empty space available */
  Rtos_ReleaseSemaphore(pFifo->hSpaceSem);
  return pElem;
}
